
#include "KisBezierUtils.h"

#include <tuple>
#include <QStack>
#include <QDebug>


#include "KisBezierPatch.h"
#include <iostream>


#ifdef HAVE_GSL
#include <gsl/gsl_multimin.h>
#endif

#include  "D://eigen_install//Eigen3//include//eigen3//Eigen//Dense"


namespace KisBezierUtils
{

    QVector<qreal> linearizeCurve(const QPointF p0, const QPointF p1, const QPointF p2, const QPointF p3, const qreal eps)
    {
        const qreal minStepSize = 2.0 / kisDistance(p0, p3);

        QVector<qreal> steps;
        steps << 0.0;


        QStack<std::tuple<QPointF, QPointF, qreal>> stackedPoints;
        stackedPoints.push(std::make_tuple(p3, 3 * (p3 - p2), 1.0));

        QPointF lastP = p0;
        QPointF lastD = 3 * (p1 - p0);
        qreal lastT = 0.0;

        while (!stackedPoints.isEmpty()) {
            QPointF p = std::get<0>(stackedPoints.top());
            QPointF d = std::get<1>(stackedPoints.top());
            qreal t = std::get<2>(stackedPoints.top());

            if (t - lastT < minStepSize ||
                isLinearSegmentByDerivatives(lastP, lastD, p, d, eps)) {

                lastP = p;
                lastD = d;
                lastT = t;
                steps << t;
                stackedPoints.pop();
            }
            else {
                t = 0.5 * (lastT + t);
                p = bezierCurve(p0, p1, p2, p3, t);
                d = bezierCurveDeriv(p0, p1, p2, p3, t);

                stackedPoints.push(std::make_tuple(p, d, t));
            }
        }

        return steps;
    }

    QVector<qreal> mergeLinearizationSteps(const QVector<qreal>& a, const QVector<qreal>& b)
    {
        QVector<qreal> result;

        std::merge(a.constBegin(), a.constEnd(),
            b.constBegin(), b.constEnd(),
            std::back_inserter(result));
        result.erase(
            std::unique(result.begin(), result.end(),
                [](qreal x, qreal y) { return qFuzzyCompare(x, y); }),
            result.end());

        return result;
    }

    class BezierSegment
    {
    private:
        /// Maximal recursion depth for finding root params
        const int MaxRecursionDepth = 64;
        /// Flatness tolerance for finding root params
        const qreal FlatnessTolerance = ldexp(1.0, -MaxRecursionDepth - 1);

    public:
        BezierSegment(int degree = 0, QPointF* p = 0)
        {
            if (degree) {
                for (int i = 0; i <= degree; ++i)
                    points.append(p[i]);
            }
        }

        void setDegree(int degree)
        {
            points.clear();
            if (degree) {
                for (int i = 0; i <= degree; ++i)
                    points.append(QPointF());
            }
        }

        int degree() const
        {
            return points.count() - 1;
        }

        QPointF point(int index) const
        {
            if (static_cast<int>(index) > degree())
                return QPointF();

            return points[index];
        }

        void setPoint(int index, const QPointF& p)
        {
            if (static_cast<int>(index) > degree())
                return;

            points[index] = p;
        }

        QPointF evaluate(qreal t, BezierSegment* left, BezierSegment* right) const
        {
            int deg = degree();
            if (!deg)
                return QPointF();

            QVector<QVector<QPointF> > Vtemp(deg + 1);
            for (int i = 0; i <= deg; ++i)
                Vtemp[i].resize(deg + 1);

            /* Copy control points  */
            for (int j = 0; j <= deg; j++) {
                Vtemp[0][j] = points[j];
            }

            /* Triangle computation */
            for (int i = 1; i <= deg; i++) {
                for (int j = 0; j <= deg - i; j++) {
                    Vtemp[i][j].rx() = (1.0 - t) * Vtemp[i - 1][j].x() + t * Vtemp[i - 1][j + 1].x();
                    Vtemp[i][j].ry() = (1.0 - t) * Vtemp[i - 1][j].y() + t * Vtemp[i - 1][j + 1].y();
                }
            }

            if (left) {
                left->setDegree(deg);
                for (int j = 0; j <= deg; j++) {
                    left->setPoint(j, Vtemp[j][0]);
                }
            }
            if (right) {
                right->setDegree(deg);
                for (int j = 0; j <= deg; j++) {
                    right->setPoint(j, Vtemp[deg - j][j]);
                }
            }

            return (Vtemp[deg][0]);
        }

        QList<qreal> roots(int depth = 0) const
        {
            QList<qreal> rootParams;

            if (!degree())
                return rootParams;

            // Calculate how often the control polygon crosses the x-axis
            // This is the upper limit for the number of roots.
            int xAxisCrossings = controlPolygonZeros(points);

            if (!xAxisCrossings) {
                // No solutions.
                return rootParams;
            }
            else if (xAxisCrossings == 1) {
                // Exactly one solution.

                // Stop recursion when the tree is deep enough
                if (depth >= MaxRecursionDepth) {
                    // if deep enough, return 1 solution at midpoint
                    rootParams.append((points.first().x() + points.last().x()) / 2.0);
                    return rootParams;
                }
                else if (isFlat(FlatnessTolerance)) {
                    // Calculate intersection of chord with x-axis.
                    QPointF chord = points.last() - points.first();
                    QPointF segStart = points.first();
                    rootParams.append((chord.x() * segStart.y() - chord.y() * segStart.x()) / -chord.y());
                    return rootParams;
                }
            }

            // Many solutions. Do recursive midpoint subdivision.
            BezierSegment left, right;
            evaluate(0.5, &left, &right);
            rootParams += left.roots(depth + 1);
            rootParams += right.roots(depth + 1);

            return rootParams;
        }

        static uint controlPolygonZeros(const QList<QPointF>& controlPoints)
        {
            int controlPointCount = controlPoints.count();
            if (controlPointCount < 2)
                return 0;

            int signChanges = 0;

            int currSign = controlPoints[0].y() < 0.0 ? -1 : 1;
            int oldSign;

            for (short i = 1; i < controlPointCount; ++i) {
                oldSign = currSign;
                currSign = controlPoints[i].y() < 0.0 ? -1 : 1;

                if (currSign != oldSign) {
                    ++signChanges;
                }
            }


            return signChanges;
        }

        int isFlat(qreal tolerance) const
        {
            int deg = degree();

            // Find the  perpendicular distance from each interior control point to
            // the line connecting points[0] and points[degree]
            qreal* distance = new qreal[deg + 1];

            // Derive the implicit equation for line connecting first and last control points
            qreal a = points[0].y() - points[deg].y();
            qreal b = points[deg].x() - points[0].x();
            qreal c = points[0].x() * points[deg].y() - points[deg].x() * points[0].y();

            qreal abSquared = (a * a) + (b * b);

            for (int i = 1; i < deg; i++) {
                // Compute distance from each of the points to that line
                distance[i] = a * points[i].x() + b * points[i].y() + c;
                if (distance[i] > 0.0) {
                    distance[i] = (distance[i] * distance[i]) / abSquared;
                }
                if (distance[i] < 0.0) {
                    distance[i] = -((distance[i] * distance[i]) / abSquared);
                }
            }


            // Find the largest distance
            qreal max_distance_above = 0.0;
            qreal max_distance_below = 0.0;
            for (int i = 1; i < deg; i++) {
                if (distance[i] < 0.0) {
                    max_distance_below = qMin(max_distance_below, distance[i]);
                }
                if (distance[i] > 0.0) {
                    max_distance_above = qMax(max_distance_above, distance[i]);
                }
            }
            delete[] distance;

            // Implicit equation for zero line
            qreal a1 = 0.0;
            qreal b1 = 1.0;
            qreal c1 = 0.0;

            // Implicit equation for "above" line
            qreal a2 = a;
            qreal b2 = b;
            qreal c2 = c + max_distance_above;

            qreal det = a1 * b2 - a2 * b1;
            qreal dInv = 1.0 / det;

            qreal intercept_1 = (b1 * c2 - b2 * c1) * dInv;

            // Implicit equation for "below" line
            a2 = a;
            b2 = b;
            c2 = c + max_distance_below;

            det = a1 * b2 - a2 * b1;
            dInv = 1.0 / det;

            qreal intercept_2 = (b1 * c2 - b2 * c1) * dInv;

            // Compute intercepts of bounding box
            qreal left_intercept = qMin(intercept_1, intercept_2);
            qreal right_intercept = qMax(intercept_1, intercept_2);

            qreal error = 0.5 * (right_intercept - left_intercept);

            return (error < tolerance);
        }

#ifndef NDEBUG
        void printDebug() const
        {
            int index = 0;
            Q_FOREACH(const QPointF & p, points) {
                qDebug() << QString("P%1 ").arg(index++) << p;
            }
        }
#endif

    private:
        QList<QPointF> points;
    };

    qreal nearestPoint(const QList<QPointF> controlPoints, const QPointF& point, qreal* resultDistance, QPointF* resultPoint)
    {
        const int deg = controlPoints.size() - 1;

        // use shortcut for line segments
        if (deg == 1) {
            // the segments chord
            QPointF chord = controlPoints.last() - controlPoints.first();
            // the point relative to the segment
            QPointF relPoint = point - controlPoints.first();
            // project point to chord (dot product)
            qreal scale = chord.x() * relPoint.x() + chord.y() * relPoint.y();
            // normalize using the chord length
            scale /= chord.x() * chord.x() + chord.y() * chord.y();

            if (scale < 0.0) {
                return 0.0;
            }
            else if (scale > 1.0) {
                return 1.0;
            }
            else {
                return scale;
            }
        }

        /* This function solves the "nearest point on curve" problem. That means, it
        * calculates the point q (to be precise: it's parameter t) on this segment, which
        * is located nearest to the input point P.
        * The basic idea is best described (because it is freely available) in "Phoenix:
        * An Interactive Curve Design System Based on the Automatic Fitting of
        * Hand-Sketched Curves", Philip J. Schneider (Master thesis, University of
        * Washington).
        *
        * For the nearest point q = C(t) on this segment, the first derivative is
        * orthogonal to the distance vector "C(t) - P". In other words we are looking for
        * solutions of f(t) = (C(t) - P) * C'(t) = 0.
        * (C(t) - P) is a nth degree curve, C'(t) a n-1th degree curve => f(t) is a
        * (2n - 1)th degree curve and thus has up to 2n - 1 distinct solutions.
        * We solve the problem f(t) = 0 by using something called "Approximate Inversion Method".
        * Let's write f(t) explicitly (with c_i = p_i - P and d_j = p_{j+1} - p_j):
        *
        *         n                     n-1
        * f(t) = SUM c_i * B^n_i(t)  *  SUM d_j * B^{n-1}_j(t)
        *        i=0                    j=0
        *
        *         n  n-1
        *      = SUM SUM w_{ij} * B^{2n-1}_{i+j}(t)
        *        i=0 j=0
        *
        * with w_{ij} = c_i * d_j * z_{ij} and
        *
        *          BinomialCoeff(n, i) * BinomialCoeff(n - i ,j)
        * z_{ij} = -----------------------------------------------
        *                   BinomialCoeff(2n - 1, i + j)
        *
        * This Bernstein-Bezier polynom representation can now be solved for its roots.
        */

        QList<QPointF> ctlPoints = controlPoints;

        // Calculate the c_i = point(i) - P.
        QPointF* c_i = new QPointF[deg + 1];

        for (int i = 0; i <= deg; ++i) {
            c_i[i] = ctlPoints[i] - point;
        }

        // Calculate the d_j = point(j + 1) - point(j).
        QPointF* d_j = new QPointF[deg];

        for (int j = 0; j <= deg - 1; ++j) {
            d_j[j] = 3.0 * (ctlPoints[j + 1] - ctlPoints[j]);
        }

        // Calculate the dot products of c_i and d_i.
        qreal* products = new qreal[deg * (deg + 1)];

        for (int j = 0; j <= deg - 1; ++j) {
            for (int i = 0; i <= deg; ++i) {
                products[j * (deg + 1) + i] = d_j[j].x() * c_i[i].x() + d_j[j].y() * c_i[i].y();
            }
        }

        // We don't need the c_i and d_i anymore.
        delete[] d_j;
        delete[] c_i;

        // Calculate the control points of the new 2n-1th degree curve.
        BezierSegment newCurve;
        newCurve.setDegree(2 * deg - 1);
        // Set up control points in the (u, f(u))-plane.
        for (unsigned short u = 0; u <= 2 * deg - 1; ++u) {
            newCurve.setPoint(u, QPointF(static_cast<qreal>(u) / static_cast<qreal>(2 * deg - 1), 0.0));
        }

        // Precomputed "z" for cubics
        static const qreal z3[3 * 4] = { 1.0, 0.6, 0.3, 0.1, 0.4, 0.6, 0.6, 0.4, 0.1, 0.3, 0.6, 1.0 };
        // Precomputed "z" for quadrics
        static const qreal z2[2 * 3] = { 1.0, 2. / 3., 1. / 3., 1. / 3., 2. / 3., 1.0 };

        const qreal* z = deg == 3 ? z3 : z2;

        // Set f(u)-values.
        for (int k = 0; k <= 2 * deg - 1; ++k) {
            int min = qMin(k, deg);

            for (unsigned short i = qMax(0, k - (deg - 1)); i <= min; ++i) {
                unsigned short j = k - i;

                // p_k += products[j][i] * z[j][i].
                QPointF currentPoint = newCurve.point(k);
                currentPoint.ry() += products[j * (deg + 1) + i] * z[j * (deg + 1) + i];
                newCurve.setPoint(k, currentPoint);
            }
        }

        // We don't need the c_i/d_i dot products and the z_{ij} anymore.
        delete[] products;

        // Find roots.
        QList<qreal> rootParams = newCurve.roots();

        // Now compare the distances of the candidate points.

        // First candidate is the previous knot.
        qreal distanceSquared = kisSquareDistance(point, controlPoints.first());
        qreal minDistanceSquared = distanceSquared;
        qreal resultParam = 0.0;
        if (resultDistance) {
            *resultDistance = std::sqrt(distanceSquared);
        }
        if (resultPoint) {
            *resultPoint = controlPoints.first();
        }

        // Iterate over the found candidate params.
        Q_FOREACH(qreal root, rootParams) {
            const QPointF rootPoint = bezierCurve(controlPoints, root);
            distanceSquared = kisSquareDistance(point, rootPoint);

            if (distanceSquared < minDistanceSquared) {
                minDistanceSquared = distanceSquared;
                resultParam = root;
                if (resultDistance) {
                    *resultDistance = std::sqrt(distanceSquared);
                }
                if (resultPoint) {
                    *resultPoint = rootPoint;
                }
            }
        }

        // Last candidate is the knot.
        distanceSquared = kisSquareDistance(point, controlPoints.last());
        if (distanceSquared < minDistanceSquared) {
            minDistanceSquared = distanceSquared;
            resultParam = 1.0;
            if (resultDistance) {
                *resultDistance = std::sqrt(distanceSquared);
            }
            if (resultPoint) {
                *resultPoint = controlPoints.last();
            }
        }

        return resultParam;
    }

    int controlPolygonZeros(const QList<QPointF>& controlPoints)
    {
        return static_cast<int>(BezierSegment::controlPolygonZeros(controlPoints));
    }



        struct Params2D {
            QPointF p0, p1, p2, p3; // top curve
            QPointF q0, q1, q2, q3; // bottom curve
            QPointF r0, r1, r2, r3; // left curve
            QPointF s0, s1, s2, s3; // right curve

            QPointF dstPoint;
        };

        struct LevelBasedPatchMethod
        {
            LevelBasedPatchMethod(qreal u, qreal v, const Params2D& p)
            {
                M_3 << 1, 0, 0, 0,
                    -3, 3, 0, 0,
                    3, -6, 3, 0,
                    -1, 3, -3, 1;

                M_3rel2abs << 1, 0, 0, 0,
                    1, 1, 0, 0,
                    0, 0, 1, 1,
                    0, 0, 0, 1;

                M_1 << -1, 1, 0, 0,
                    1, -1, -1, 1;

                PQ_left << p.p0.x(), p.p0.y(),
                    p.p1.x(), p.p1.y(),
                    p.q0.x(), p.q0.y(),
                    p.q1.x(), p.q1.y();

                PQ_right << p.p3.x(), p.p3.y(),
                    p.p2.x(), p.p2.y(),
                    p.q3.x(), p.q3.y(),
                    p.q2.x(), p.q2.y();

                RS_top << p.r0.x(), p.r0.y(),
                    p.r1.x(), p.r1.y(),
                    p.s0.x(), p.s0.y(),
                    p.s1.x(), p.s1.y();

                RS_bottom << p.r3.x(), p.r3.y(),
                    p.r2.x(), p.r2.y(),
                    p.s3.x(), p.s3.y(),
                    p.s2.x(), p.s2.y();

                R << p.r0.x(), p.r0.y(),
                    p.r1.x(), p.r1.y(),
                    p.r2.x(), p.r2.y(),
                    p.r3.x(), p.r3.y();

                S << p.s0.x(), p.s0.y(),
                    p.s1.x(), p.s1.y(),
                    p.s2.x(), p.s2.y(),
                    p.s3.x(), p.s3.y();

                P << p.p0.x(), p.p0.y(),
                    p.p1.x(), p.p1.y(),
                    p.p2.x(), p.p2.y(),
                    p.p3.x(), p.p3.y();

                Q << p.q0.x(), p.q0.y(),
                    p.q1.x(), p.q1.y(),
                    p.q2.x(), p.q2.y(),
                    p.q3.x(), p.q3.y();

                T_u3 << 1, u, pow2(u), pow3(u);
                T_v3 << 1, v, pow2(v), pow3(v);
                T_dot_u3 << 0, 1, 2 * u, 3 * pow2(u);
                T_dot_v3 << 0, 1, 2 * v, 3 * pow2(v);

                T_u1 << 1, u;
                T_v1 << 1, v;
                T_dot_u1 << 0, 1;
                T_dot_v1 << 0, 1;
            }

            Eigen::Matrix4d M_3;
            Eigen::Matrix4d M_3rel2abs;
            Eigen::Matrix<double, 2, 4> M_1;


            Eigen::Matrix<double, 4, 2> PQ_left;
            Eigen::Matrix<double, 4, 2> PQ_right;
            Eigen::Matrix<double, 4, 2> RS_top;
            Eigen::Matrix<double, 4, 2> RS_bottom;

            Eigen::Matrix<double, 4, 2> R;
            Eigen::Matrix<double, 4, 2> S;
            Eigen::Matrix<double, 4, 2> P;
            Eigen::Matrix<double, 4, 2> Q;

            Eigen::Matrix<double, 1, 4> T_u3;
            Eigen::Matrix<double, 1, 4> T_v3;
            Eigen::Matrix<double, 1, 4> T_dot_u3;
            Eigen::Matrix<double, 1, 4> T_dot_v3;

            Eigen::Matrix<double, 1, 2> T_u1;
            Eigen::Matrix<double, 1, 2> T_v1;
            Eigen::Matrix<double, 1, 2> T_dot_u1;
            Eigen::Matrix<double, 1, 2> T_dot_v1;

            QPointF value() const {
                Eigen::Matrix<double, 1, 2> L_1 = T_v3 * M_3 * R;
                Eigen::Matrix<double, 1, 2> L_2 = T_v1 * M_1 * PQ_left;
                Eigen::Matrix<double, 1, 2> L_3 = T_v1 * M_1 * PQ_right;
                Eigen::Matrix<double, 1, 2> L_4 = T_v3 * M_3 * S;

                Eigen::Matrix<double, 4, 2> L_controls;
                L_controls << L_1, L_2, L_3, L_4;

                Eigen::Matrix<double, 1, 2> L = T_u3 * M_3 * M_3rel2abs * L_controls;


                Eigen::Matrix<double, 1, 2> H_1 = T_u3 * M_3 * P;
                Eigen::Matrix<double, 1, 2> H_2 = T_u1 * M_1 * RS_top;
                Eigen::Matrix<double, 1, 2> H_3 = T_u1 * M_1 * RS_bottom;
                Eigen::Matrix<double, 1, 2> H_4 = T_u3 * M_3 * Q;

                Eigen::Matrix<double, 4, 2> H_controls;
                H_controls << H_1, H_2, H_3, H_4;

                Eigen::Matrix<double, 1, 2> H = T_v3 * M_3 * M_3rel2abs * H_controls;

                Eigen::Matrix<double, 1, 2> result = 0.5 * (L + H);

                return QPointF(result(0, 0), result(0, 1));
            }

            QPointF diffU() const {
                Eigen::Matrix<double, 1, 2> L_1 = T_v3 * M_3 * R;
                Eigen::Matrix<double, 1, 2> L_2 = T_v1 * M_1 * PQ_left;
                Eigen::Matrix<double, 1, 2> L_3 = T_v1 * M_1 * PQ_right;
                Eigen::Matrix<double, 1, 2> L_4 = T_v3 * M_3 * S;

                Eigen::Matrix<double, 4, 2> L_controls;
                L_controls << L_1, L_2, L_3, L_4;

                Eigen::Matrix<double, 1, 2> L = T_dot_u3 * M_3 * M_3rel2abs * L_controls;


                Eigen::Matrix<double, 1, 2> H_1 = T_dot_u3 * M_3 * P;
                Eigen::Matrix<double, 1, 2> H_2 = T_dot_u1 * M_1 * RS_top;
                Eigen::Matrix<double, 1, 2> H_3 = T_dot_u1 * M_1 * RS_bottom;
                Eigen::Matrix<double, 1, 2> H_4 = T_dot_u3 * M_3 * Q;

                Eigen::Matrix<double, 4, 2> H_controls;
                H_controls << H_1, H_2, H_3, H_4;

                Eigen::Matrix<double, 1, 2> H = T_v3 * M_3 * M_3rel2abs * H_controls;

                Eigen::Matrix<double, 1, 2> result = 0.5 * (L + H);
                return QPointF(result(0, 0), result(0, 1));
            }

            QPointF diffV() const {
                Eigen::Matrix<double, 1, 2> L_1 = T_dot_v3 * M_3 * R;
                Eigen::Matrix<double, 1, 2> L_2 = T_dot_v1 * M_1 * PQ_left;
                Eigen::Matrix<double, 1, 2> L_3 = T_dot_v1 * M_1 * PQ_right;
                Eigen::Matrix<double, 1, 2> L_4 = T_dot_v3 * M_3 * S;

                Eigen::Matrix<double, 4, 2> L_controls;
                L_controls << L_1, L_2, L_3, L_4;

                Eigen::Matrix<double, 1, 2> L = T_u3 * M_3 * M_3rel2abs * L_controls;

                Eigen::Matrix<double, 1, 2> H_1 = T_u3 * M_3 * P;
                Eigen::Matrix<double, 1, 2> H_2 = T_u1 * M_1 * RS_top;
                Eigen::Matrix<double, 1, 2> H_3 = T_u1 * M_1 * RS_bottom;
                Eigen::Matrix<double, 1, 2> H_4 = T_u3 * M_3 * Q;

                Eigen::Matrix<double, 4, 2> H_controls;
                H_controls << H_1, H_2, H_3, H_4;

                Eigen::Matrix<double, 1, 2> H = T_dot_v3 * M_3 * M_3rel2abs * H_controls;

                Eigen::Matrix<double, 1, 2> result = 0.5 * (L + H);

                return QPointF(result(0, 0), result(0, 1));
            }

        };

    //    struct SvgPatchMethod
    //    {
    //    private:

    //        /**
    //         * TODO: optimize these function somehow!
    //         */

    //        static QPointF meshForwardMapping(qreal u, qreal v, const Params2D& p) {
    //            return p.r0 + pow3(u) * v * (p.p0 - 3 * p.p1 + 3 * p.p2 - p.p3 - p.q0 + 3 * p.q1 - 3 * p.q2 + p.q3) + pow3(u) * (-p.p0 + 3 * p.p1 - 3 * p.p2 + p.p3) + pow2(u) * v * (-3 * p.p0 + 6 * p.p1 - 3 * p.p2 + 3 * p.q0 - 6 * p.q1 + 3 * p.q2) + pow2(u) * (3 * p.p0 - 6 * p.p1 + 3 * p.p2) + u * pow3(v) * (p.r0 - 3 * p.r1 + 3 * p.r2 - p.r3 - p.s0 + 3 * p.s1 - 3 * p.s2 + p.s3) + u * pow2(v) * (-3 * p.r0 + 6 * p.r1 - 3 * p.r2 + 3 * p.s0 - 6 * p.s1 + 3 * p.s2) + u * v * (2 * p.p0 - 3 * p.p1 + p.p3 - 2 * p.q0 + 3 * p.q1 - p.q3 + 3 * p.r0 - 3 * p.r1 - 3 * p.s0 + 3 * p.s1) + u * (-2 * p.p0 + 3 * p.p1 - p.p3 - p.r0 + p.s0) + pow3(v) * (-p.r0 + 3 * p.r1 - 3 * p.r2 + p.r3) + pow2(v) * (3 * p.r0 - 6 * p.r1 + 3 * p.r2) + v * (-3 * p.r0 + 3 * p.r1);
    //        }

    //        static QPointF meshForwardMappingDiffU(qreal u, qreal v, const Params2D& p) {
    //            return -2 * p.p0 + 3 * p.p1 - p.p3 - p.r0 + p.s0 + pow2(u) * v * (3 * p.p0 - 9 * p.p1 + 9 * p.p2 - 3 * p.p3 - 3 * p.q0 + 9 * p.q1 - 9 * p.q2 + 3 * p.q3) + pow2(u) * (-3 * p.p0 + 9 * p.p1 - 9 * p.p2 + 3 * p.p3) + u * v * (-6 * p.p0 + 12 * p.p1 - 6 * p.p2 + 6 * p.q0 - 12 * p.q1 + 6 * p.q2) + u * (6 * p.p0 - 12 * p.p1 + 6 * p.p2) + pow3(v) * (p.r0 - 3 * p.r1 + 3 * p.r2 - p.r3 - p.s0 + 3 * p.s1 - 3 * p.s2 + p.s3) + pow2(v) * (-3 * p.r0 + 6 * p.r1 - 3 * p.r2 + 3 * p.s0 - 6 * p.s1 + 3 * p.s2) + v * (2 * p.p0 - 3 * p.p1 + p.p3 - 2 * p.q0 + 3 * p.q1 - p.q3 + 3 * p.r0 - 3 * p.r1 - 3 * p.s0 + 3 * p.s1);
    //        }

    //        static QPointF meshForwardMappingDiffV(qreal u, qreal v, const Params2D& p) {
    //            return -3 * p.r0 + 3 * p.r1 + pow3(u) * (p.p0 - 3 * p.p1 + 3 * p.p2 - p.p3 - p.q0 + 3 * p.q1 - 3 * p.q2 + p.q3) + pow2(u) * (-3 * p.p0 + 6 * p.p1 - 3 * p.p2 + 3 * p.q0 - 6 * p.q1 + 3 * p.q2) + u * pow2(v) * (3 * p.r0 - 9 * p.r1 + 9 * p.r2 - 3 * p.r3 - 3 * p.s0 + 9 * p.s1 - 9 * p.s2 + 3 * p.s3) + u * v * (-6 * p.r0 + 12 * p.r1 - 6 * p.r2 + 6 * p.s0 - 12 * p.s1 + 6 * p.s2) + u * (2 * p.p0 - 3 * p.p1 + p.p3 - 2 * p.q0 + 3 * p.q1 - p.q3 + 3 * p.r0 - 3 * p.r1 - 3 * p.s0 + 3 * p.s1) + pow2(v) * (-3 * p.r0 + 9 * p.r1 - 9 * p.r2 + 3 * p.r3) + v * (6 * p.r0 - 12 * p.r1 + 6 * p.r2);
    //        }

    //        qreal u = 0.0;
    //        qreal v = 0.0;
    //        const Params2D p;

    //    public:
    //        SvgPatchMethod(qreal _u, qreal _v, const Params2D& _p)
    //            : u(_u), v(_v), p(_p)
    //        {
    //        }

    //        QPointF value() const {
    //            return meshForwardMapping(u, v, p);
    //        }

    //        QPointF diffU() const {
    //            return meshForwardMappingDiffU(u, v, p);
    //        }

    //        QPointF diffV() const {
    //            return meshForwardMappingDiffV(u, v, p);
    //        }

    //    };

    //    template <class PatchMethod>
    //    double my_f(const gsl_vector* x, void* paramsPtr)
    //    {
    //        const Params2D* params = static_cast<const Params2D*>(paramsPtr);
    //        const QPointF pos(gsl_vector_get(x, 0), gsl_vector_get(x, 1));

    //        PatchMethod mat(pos.x(), pos.y(), *params);
    //        const QPointF S = mat.value();

    //        return kisSquareDistance(S, params->dstPoint);
    //    }

    //    template <class PatchMethod>
    //    void my_fdf(const gsl_vector* x, void* paramsPtr, double* f, gsl_vector* df)
    //    {
    //        const Params2D* params = static_cast<const Params2D*>(paramsPtr);
    //        const QPointF pos(gsl_vector_get(x, 0), gsl_vector_get(x, 1));

    //        PatchMethod mat(pos.x(), pos.y(), *params);
    //        const QPointF S = mat.value();
    //        const QPointF dU = mat.diffU();
    //        const QPointF dV = mat.diffV();

    //        *f = kisSquareDistance(S, params->dstPoint);

    //        gsl_vector_set(df, 0,
    //            2 * (S.x() - params->dstPoint.x()) * dU.x() +
    //            2 * (S.y() - params->dstPoint.y()) * dU.y());
    //        gsl_vector_set(df, 1,
    //            2 * (S.x() - params->dstPoint.x()) * dV.x() +
    //            2 * (S.y() - params->dstPoint.y()) * dV.y());
    //    }

    //    template <class PatchMethod>
    //    void my_df(const gsl_vector* x, void* paramsPtr,
    //        gsl_vector* df)
    //    {
    //        const Params2D* params = static_cast<const Params2D*>(paramsPtr);
    //        const QPointF pos(gsl_vector_get(x, 0), gsl_vector_get(x, 1));

    //        PatchMethod mat(pos.x(), pos.y(), *params);
    //        const QPointF S = mat.value();
    //        const QPointF dU = mat.diffU();
    //        const QPointF dV = mat.diffV();

    //        gsl_vector_set(df, 0,
    //            2 * (S.x() - params->dstPoint.x()) * dU.x() +
    //            2 * (S.y() - params->dstPoint.y()) * dU.y());
    //        gsl_vector_set(df, 1,
    //            2 * (S.x() - params->dstPoint.x()) * dV.x() +
    //            2 * (S.y() - params->dstPoint.y()) * dV.y());
    //    }
    //}






    template <class PatchMethod>
    QPointF calculateLocalPosImpl(const std::array<QPointF, 12>& points, const QPointF& globalPoint)
    {
        QRectF patchBounds;

        for (auto it = points.begin(); it != points.end(); ++it) {
            KisAlgebra2D::accumulateBounds(*it, &patchBounds);
        }

        const QPointF approxStart = KisAlgebra2D::absoluteToRelative(globalPoint, patchBounds);
        //risk
        //KIS_SAFE_ASSERT_RECOVER_NOOP(QRectF(0, 0, 1.0, 1.0).contains(approxStart));

#ifdef HAVE_GSL
        const gsl_multimin_fdfminimizer_type* T =
            gsl_multimin_fdfminimizer_vector_bfgs2;
        gsl_multimin_fdfminimizer* s = 0;
        gsl_vector* x;
        gsl_multimin_function_fdf minex_func;

        size_t iter = 0;
        int status;

        /* Starting point */
        x = gsl_vector_alloc(2);
        gsl_vector_set(x, 0, approxStart.x());
        gsl_vector_set(x, 1, approxStart.y());

        Params2D p;

        p.p0 = points[KisBezierPatch::TL];
        p.p1 = points[KisBezierPatch::TL_HC];
        p.p2 = points[KisBezierPatch::TR_HC];
        p.p3 = points[KisBezierPatch::TR];

        p.q0 = points[KisBezierPatch::BL];
        p.q1 = points[KisBezierPatch::BL_HC];
        p.q2 = points[KisBezierPatch::BR_HC];
        p.q3 = points[KisBezierPatch::BR];

        p.r0 = points[KisBezierPatch::TL];
        p.r1 = points[KisBezierPatch::TL_VC];
        p.r2 = points[KisBezierPatch::BL_VC];
        p.r3 = points[KisBezierPatch::BL];

        p.s0 = points[KisBezierPatch::TR];
        p.s1 = points[KisBezierPatch::TR_VC];
        p.s2 = points[KisBezierPatch::BR_VC];
        p.s3 = points[KisBezierPatch::BR];

        p.dstPoint = globalPoint;

        /* Initialize method and iterate */
        minex_func.n = 2;
        minex_func.f = my_f<PatchMethod>;
        minex_func.params = (void*)&p;
        minex_func.df = my_df<PatchMethod>;
        minex_func.fdf = my_fdf<PatchMethod>;

        s = gsl_multimin_fdfminimizer_alloc(T, 2);
        gsl_multimin_fdfminimizer_set(s, &minex_func, x, 0.01, 0.1);

        QPointF result;


        result.rx() = gsl_vector_get(s->x, 0);
        result.ry() = gsl_vector_get(s->x, 1);

        do
        {
            iter++;
            status = gsl_multimin_fdfminimizer_iterate(s);

            if (status)
                break;

            status = gsl_multimin_test_gradient(s->gradient, 1e-4);

            result.rx() = gsl_vector_get(s->x, 0);
            result.ry() = gsl_vector_get(s->x, 1);

            if (status == GSL_SUCCESS)
            {
                result.rx() = gsl_vector_get(s->x, 0);
                result.ry() = gsl_vector_get(s->x, 1);
                //qDebug() << "******* Converged to minimum" << ppVar(result);

            }
        } while (status == GSL_CONTINUE && iter < 10000);

        //    ENTER_FUNCTION()<< ppVar(iter) << ppVar(globalPoint) << ppVar(result);
        //    ENTER_FUNCTION() << ppVar(meshForwardMapping(result.x(), result.y(), p));

        gsl_vector_free(x);
        gsl_multimin_fdfminimizer_free(s);

        return result;
#else
        return approxStart;
#endif
    }

    template <class PatchMethod>
    QPointF calculateGlobalPosImpl(const std::array<QPointF, 12>& points, const QPointF& localPoint)
    {
        Params2D p;

        p.p0 = points[KisBezierPatch::TL];
        p.p1 = points[KisBezierPatch::TL_HC];
        p.p2 = points[KisBezierPatch::TR_HC];
        p.p3 = points[KisBezierPatch::TR];

        p.q0 = points[KisBezierPatch::BL];
        p.q1 = points[KisBezierPatch::BL_HC];
        p.q2 = points[KisBezierPatch::BR_HC];
        p.q3 = points[KisBezierPatch::BR];

        p.r0 = points[KisBezierPatch::TL];
        p.r1 = points[KisBezierPatch::TL_VC];
        p.r2 = points[KisBezierPatch::BL_VC];
        p.r3 = points[KisBezierPatch::BL];

        p.s0 = points[KisBezierPatch::TR];
        p.s1 = points[KisBezierPatch::TR_VC];
        p.s2 = points[KisBezierPatch::BR_VC];
        p.s3 = points[KisBezierPatch::BR];

        PatchMethod f(localPoint.x(), localPoint.y(), p);
        return f.value();
    }

    QPointF calculateLocalPos(const std::array<QPointF, 12>& points, const QPointF& globalPoint)
    {
        return calculateLocalPosImpl<LevelBasedPatchMethod>(points, globalPoint);
    }

    QPointF calculateGlobalPos(const std::array<QPointF, 12>& points, const QPointF& localPoint)
    {
        return calculateGlobalPosImpl<LevelBasedPatchMethod>(points, localPoint);
    }

    //QPointF calculateLocalPosSVG2(const std::array<QPointF, 12>& points, const QPointF& globalPoint)
    //{
    //    return calculateLocalPosImpl<SvgPatchMethod>(points, globalPoint);
    //}

    //QPointF calculateGlobalPosSVG2(const std::array<QPointF, 12>& points, const QPointF& localPoint)
    //{
    //    return calculateGlobalPosImpl<SvgPatchMethod>(points, localPoint);
    //}

    QPointF interpolateQuadric(const QPointF& p0, const QPointF& p2, const QPointF& pt, qreal t)
    {
        if (t <= 0.0 || t >= 1.0)
            return lerp(p0, p2, 0.5);

        /*
            B(t) = [x2 y2] = (1-t)^2*P0 + 2t*(1-t)*P1 + t^2*P2

                   B(t) - (1-t)^2*P0 - t^2*P2
             P1 =  --------------------------
                           2t*(1-t)
        */

        QPointF c1 = pt - (1.0 - t) * (1.0 - t) * p0 - t * t * p2;

        qreal denom = 2.0 * t * (1.0 - t);

        c1.rx() /= denom;
        c1.ry() /= denom;

        return c1;
    }

    std::pair<QPointF, QPointF> offsetSegment(qreal t, const QPointF& offset)
    {
        /*
        * method from inkscape, original method and idea borrowed from Simon Budig
        * <simon@gimp.org> and the GIMP
        * cf. app/vectors/gimpbezierstroke.c, gimp_bezier_stroke_point_move_relative()
        *
        * feel good is an arbitrary parameter that distributes the delta between handles
        * if t of the drag point is less than 1/6 distance form the endpoint only
        * the corresponding handle is adjusted. This matches the behavior in GIMP
        */
        qreal feel_good;
        if (t <= 1.0 / 6.0)
            feel_good = 0;
        else if (t <= 0.5)
            feel_good = (pow((6 * t - 1) / 2.0, 3)) / 2;
        else if (t <= 5.0 / 6.0)
            feel_good = (1 - pow((6 * (1 - t) - 1) / 2.0, 3)) / 2 + 0.5;
        else
            feel_good = 1;

        const QPointF moveP1 = ((1 - feel_good) / (3 * t * (1 - t) * (1 - t))) * offset;
        const QPointF moveP2 = (feel_good / (3 * t * t * (1 - t))) * offset;

        return std::make_pair(moveP1, moveP2);
    }

    qreal curveLength(const QPointF p0, const QPointF p1, const QPointF p2, const QPointF p3, const qreal error)
    {
        /*
         * This algorithm is implemented based on an idea by Jens Gravesen:
         * "Adaptive subdivision and the length of Bezier curves" mat-report no. 1992-10, Mathematical Institute,
         * The Technical University of Denmark.
         *
         * By subdividing the curve at parameter value t you only have to find the length of a full Bezier curve.
         * If you denote the length of the control polygon by L1 i.e.:
         *   L1 = |P0 P1| +|P1 P2| +|P2 P3|
         *
         * and the length of the cord by L0 i.e.:
         *   L0 = |P0 P3|
         *
         * then
         *   L = 1/2*L0 + 1/2*L1
         *
         * is a good approximation to the length of the curve, and the difference
         *   ERR = L1-L0
         *
         * is a measure of the error. If the error is to large, then you just subdivide curve at parameter value
         * 1/2, and find the length of each half.
         * If m is the number of subdivisions then the error goes to zero as 2^-4m.
         * If you don't have a cubic curve but a curve of degree n then you put
         *   L = (2*L0 + (n-1)*L1)/(n+1)
         */

        const int deg = bezierDegree(p0, p1, p2, p3);

        if (deg == -1)
            return 0.0;

        // calculate chord length
        const qreal chordLen = kisDistance(p0, p3);

        if (deg == 1) {
            return chordLen;
        }

        // calculate length of control polygon
        qreal polyLength = 0.0;

        polyLength += kisDistance(p0, p1);
        polyLength += kisDistance(p1, p2);
        polyLength += kisDistance(p2, p3);

        if ((polyLength - chordLen) > error) {
            QPointF q0, q1, q2, q3, q4;
            deCasteljau(p0, p1, p2, p3, 0.5, &q0, &q1, &q2, &q3, &q4);

            return curveLength(p0, q0, q1, q2, error) +
                curveLength(q2, q3, q4, p3, error);
        }
        else {
            // the error is smaller than our tolerance
            if (deg == 3)
                return 0.5 * chordLen + 0.5 * polyLength;
            else
                return (2.0 * chordLen + polyLength) / 3.0;
        }
    }

    qreal curveLengthAtPoint(const QPointF p0, const QPointF p1, const QPointF p2, const QPointF p3, qreal t, const qreal error)
    {
        QPointF q0, q1, q2, q3, q4;
        deCasteljau(p0, p1, p2, p3, t, &q0, &q1, &q2, &q3, &q4);

        return curveLength(p0, q0, q1, q2, error);
    }

    qreal curveParamBySegmentLength(const QPointF p0, const QPointF p1, const QPointF p2, const QPointF p3, qreal expectedLength, qreal totalLength, const qreal error)
    {
        const qreal splitAtParam = expectedLength / totalLength;

        QPointF q0, q1, q2, q3, q4;
        deCasteljau(p0, p1, p2, p3, splitAtParam, &q0, &q1, &q2, &q3, &q4);

        const qreal portionLength = curveLength(p0, q0, q1, q2, error);

        if (std::abs(portionLength - expectedLength) < error) {
            return splitAtParam;
        }
        else if (portionLength < expectedLength) {
            return splitAtParam + (1.0 - splitAtParam) * curveParamBySegmentLength(q2, q3, q4, p3, expectedLength - portionLength, totalLength - portionLength, error);
        }
        else {
            return splitAtParam * curveParamBySegmentLength(p0, q0, q1, q2, expectedLength, portionLength, error);
        }
    }


    qreal curveParamByProportion(const QPointF p0, const QPointF p1, const QPointF p2, const QPointF p3, qreal proportion, const qreal error)
    {
        const qreal totalLength = curveLength(p0, p1, p2, p3, error);
        const qreal expectedLength = proportion * totalLength;

        return curveParamBySegmentLength(p0, p1, p2, p3, expectedLength, totalLength, error);
    }

    qreal curveProportionByParam(const QPointF p0, const QPointF p1, const QPointF p2, const QPointF p3, qreal t, const qreal error)
    {
        return curveLengthAtPoint(p0, p1, p2, p3, t, error) / curveLength(p0, p1, p2, p3, error);
    }

    std::pair<QPointF, QPointF> removeBezierNode(const QPointF& p0,
        const QPointF& p1,
        const QPointF& p2,
        const QPointF& p3,
        const QPointF& q1,
        const QPointF& q2,
        const QPointF& q3)
    {
        /**
         * Calculates the curve control point after removal of a node
         * by minimizing squared error for the folloing problem:
         *
         * Given two consequent 3rd order curves P and Q with lengths Lp and Lq,
         * find 3rd order curve B, so that when splitting it at t = Lp / (Lp + Lq)
         * (using de Casteljau algorithm) the control points of the resulting
         * curves will have least square errors, compared to the corresponding
         * control points of curves P and Q.
         *
         * First we represent curves in matrix form:
         *
         * B(t) = T * M * B,
         * P(t) = T * Z1 * M * P,
         * Q(t) = T * Z2 * M * Q,
         *
         * where
         *    T = [1, t, t^2, t^3]
         *    M --- 4x4 matrix of Bezier coefficients
         *    B, P, Q --- vector of control points for the curves
         *
         *    Z1 --- conversion matrix that splits the curve into range [0.0...t]
         *    Z2 --- conversion matrix that splits the curve into range [t...1.0]
         *
         * Then we represent vectors P and Q via B:
         *
         * P = M^(-1) * Z1 * M * B
         * Q = M^(-1) * Z2 * M * B
         *
         * which in block matrix form looks like:
         *
         * [ P ]   [ M^(-1) * Z1 * M ]
         * |   | = |                 | * B
         * [ Q ]   [ M^(-1) * Z2 * M ]
         *
         *
         *         [ M^(-1) * Z1 * M ]
         * let C = |                 |,
         *         [ M^(-1) * Z2 * M ]
         *
         *     [ P ]
         * R = |   |
         *     [ Q ]
         *
         *
         * then
         *
         * R = C * B
         *
         * applying normal equation to find a solution with least square error,
         * get the final result:
         *
         * B = (C'C)^(-1) * C' * R
         */

        const qreal lenP = KisBezierUtils::curveLength(p0, p1, p2, p3, 0.001);
        const qreal lenQ = KisBezierUtils::curveLength(p3, q1, q2, q3, 0.001);

        const qreal z = lenP / (lenP + lenQ);

        Eigen::Matrix4f M;
        M << 1, 0, 0, 0,
            -3, 3, 0, 0,
            3, -6, 3, 0,
            -1, 3, -3, 1;

        Eigen::DiagonalMatrix<float, 4> Z_1;
        Z_1.diagonal() << 1, z, pow2(z), pow3(z);

        Eigen::Matrix4f Z_2;
        Z_2 << 1, z, pow2(z), pow3(z),
            0, 1 - z, 2 * z * (1 - z), 3 * pow2(z) * (1 - z),
            0, 0, pow2(1 - z), 3 * z * pow2(1 - z),
            0, 0, 0, pow3(1 - z);

        Eigen::Matrix<float, 8, 2> R;
        R << p0.x(), p0.y(),
            p1.x(), p1.y(),
            p2.x(), p2.y(),
            p3.x(), p3.y(),
            p3.x(), p3.y(),
            q1.x(), q1.y(),
            q2.x(), q2.y(),
            q3.x(), q3.y();

        Eigen::Matrix<float, 2, 2> B_const;
        B_const << p0.x(), p0.y(),
            q3.x(), q3.y();


        Eigen::Matrix4f M1 = M.inverse() * Z_1 * M;
        Eigen::Matrix4f M2 = M.inverse() * Z_2 * M;

        Eigen::Matrix<float, 8, 4> C;
        C << M1, M2;

        Eigen::Matrix<float, 8, 2> C_const;
        C_const << C.col(0), C.col(3);

        Eigen::Matrix<float, 8, 2> C_var;
        C_var << C.col(1), C.col(2);

        Eigen::Matrix<float, 8, 2> R_var;
        R_var = R - C_const * B_const;

        Eigen::Matrix<float, 6, 2> R_reduced;
        R_reduced = R_var.block(1, 0, 6, 2);

        Eigen::Matrix<float, 6, 2> C_reduced;
        C_reduced = C_var.block(1, 0, 6, 2);

        Eigen::Matrix<float, 2, 2> result;
        result = (C_reduced.transpose() * C_reduced).inverse() * C_reduced.transpose() * R_reduced;

        QPointF resultP0(result(0, 0), result(0, 1));
        QPointF resultP1(result(1, 0), result(1, 1));

        return std::make_pair(resultP0, resultP1);
    }
    QVector<qreal> intersectWithLineImpl(const QPointF& p0, const QPointF& p1, const QPointF& p2, const QPointF& p3, const QLineF& line, qreal eps, qreal alpha, qreal beta)
    {
        using KisAlgebra2D::intersectLines;

        QVector<qreal> result;

        const qreal length =
            kisDistance(p0, p1) +
            kisDistance(p1, p2) +
            kisDistance(p2, p3);

        if (length < eps) {
            if (intersectLines(p0, p3, line.p1(), line.p2())) {
                result << alpha * 0.5 + beta;
            }
        }
        else {

            const bool hasIntersections =
                intersectLines(p0, p1, line.p1(), line.p2()) ||
                intersectLines(p1, p2, line.p1(), line.p2()) ||
                intersectLines(p2, p3, line.p1(), line.p2());

            if (hasIntersections) {
                QPointF q0, q1, q2, q3, q4;

                deCasteljau(p0, p1, p2, p3, 0.5, &q0, &q1, &q2, &q3, &q4);

                result << intersectWithLineImpl(p0, q0, q1, q2, line, eps, 0.5 * alpha, beta);
                result << intersectWithLineImpl(q2, q3, q4, p3, line, eps, 0.5 * alpha, beta + 0.5 * alpha);
            }
        }
        return result;
    }

    QVector<qreal> intersectWithLine(const QPointF& p0, const QPointF& p1, const QPointF& p2, const QPointF& p3, const QLineF& line, qreal eps)
    {
        return intersectWithLineImpl(p0, p1, p2, p3, line, eps, 1.0, 0.0);
    }

    boost::optional<qreal> intersectWithLineNearest(const QPointF& p0, const QPointF& p1, const QPointF& p2, const QPointF& p3, const QLineF& line, const QPointF& nearestAnchor, qreal eps)
    {
        QVector<qreal> result = intersectWithLine(p0, p1, p2, p3, line, eps);

        qreal minDistance = std::numeric_limits<qreal>::max();
        boost::optional<qreal> nearestRoot;

        Q_FOREACH(qreal root, result) {
            const QPointF pt = bezierCurve(p0, p1, p2, p3, root);
            const qreal distance = kisDistance(pt, nearestAnchor);

            if (distance < minDistance) {
                minDistance = distance;
                nearestRoot = root;
            }
        }

        return nearestRoot;
    }


}
